/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "StdAfx.h"
#include "ColumnVector.h"

namespace RobotPlant { namespace Math{
	ColumnVector::ColumnVector()
		{
			_count = 0;
			_datas = new double[0];
		}

	ColumnVector::ColumnVector(const int count)
	{
		_count = count;
		_datas = new double[_count];
	}

	ColumnVector::ColumnVector(const ColumnVector& arg)
	{
		_count = arg._count;
		_datas = new double[_count];

		for(int i=0; i< _count; i++)
		{
			_datas[i] = arg._datas[i];
		}
	}

	ColumnVector::ColumnVector(const Vector2& arg)
	{
		_count = 2;
		_datas = new double[_count];

		for(int i=0; i< _count; i++)
		{
			_datas[i] = arg[i];
		}
	}

	ColumnVector::ColumnVector(const Vector3& arg)
	{
		_count = 3;
		_datas = new double[_count];

		for(int i=0; i< _count; i++)
		{
			_datas[i] = arg[i];
		}
	}

	ColumnVector::ColumnVector(const Acceleration& arg)
	{
		_count = 6;
		_datas = new double[_count];

		for(int i=0; i< _count; i++)
		{
			_datas[i] = arg[i];
		}
	}

	ColumnVector::ColumnVector(const Velocity& arg)
	{
		_count = 6;
		_datas = new double[_count];

		for(int i=0; i< _count; i++)
		{
			_datas[i] = arg[i];
		}
	}

	ColumnVector::ColumnVector(const Wrench& arg)
	{
		_count = 6;
		_datas = new double[_count];

		for(int i=0; i< _count; i++)
		{
			_datas[i] = arg[i];
		}
	}

	ColumnVector::~ColumnVector(void)
	{
		delete[] _datas;
	}

	//Operator overloads.
	double ColumnVector::operator[] (const int index) const
	{
		if((index < 0) || (index >= this->_count))
			throw(new std::exception("Argument index must be >= 0 and < ColumnVector's row."));

		return this->_datas[index];
	}

	double& ColumnVector::operator[] (const int index)
	{
		if((index < 0) || (index >= this->_count))
			throw(new std::exception("Argument index must be >= 0 and < ColumnVector's row."));

		return this->_datas[index];
	}

	ColumnVector& ColumnVector::operator = ( const ColumnVector& arg)
	{
		//if(this->_count != arg._count)
		//	throw(new std::exception("This ColumnVector's count must be equal to arg's count."));
		if( _count != arg._count)
		{
			delete[] _datas;
			_count = arg._count;
			_datas = new double[_count];
		}

		for(int i=0; i< arg._count; i++)
		{
			this->_datas[i] = arg._datas[i];
		}

		return *this;
	}

	ColumnVector& ColumnVector::operator += (const ColumnVector& arg)
	{
		if(this->_count != arg._count)
			throw(new std::exception("This's count must be equal to arg's count."));

		for(int i=0; i< arg._count; i++)
		{
			this->_datas[i] = this->_datas[i] + arg._datas[i];
		}
		return *this;
	}

	ColumnVector& ColumnVector::operator -= (const ColumnVector& arg)
	{
		if(this->_count != arg._count)
			throw(new std::exception("This's count must be equal to arg's count."));

		for(int i=0; i< arg._count; i++)
		{
			this->_datas[i] = this->_datas[i] - arg._datas[i];
		}
		return *this;
	}

	bool operator == (const ColumnVector& lhs, const ColumnVector& rhs)
	{
		return Equal(lhs,rhs);
	}

	bool operator != (const ColumnVector& lhs, const ColumnVector& rhs)
	{
		return !Equal(lhs,rhs);
	}

	ColumnVector operator + (const ColumnVector& lhs,const ColumnVector& rhs)
	{
		if(lhs._count != lhs._count)
			throw(new std::exception("lhs's count must be equal to rhs's count."));

		ColumnVector res = ColumnVector(lhs);
		for(int i=0; i< res._count; i++)
		{
			res._datas[i] = res._datas[i] + rhs._datas[i];
		}
		return res;
	}

	ColumnVector operator - (const ColumnVector& lhs,const ColumnVector& rhs)
	{
		if(lhs._count != lhs._count)
			throw(new std::exception("lhs's count must be equal to rhs's count."));

		ColumnVector res = ColumnVector(lhs);
		for(int i=0; i< res._count; i++)
		{
			res._datas[i] = res._datas[i] - rhs._datas[i];
		}
		return res;
	}

	ColumnVector operator * (const ColumnVector& lhs,const double rhs)
	{
		ColumnVector ret = ColumnVector(lhs.GetCount());
		for(int i=0; i<lhs.GetCount(); i++)
		{
			ret[i] = lhs[i] * rhs;
		}

		return ret;
	}

	ColumnVector operator * (const double& lhs,const ColumnVector& rhs)
	{
		ColumnVector ret = ColumnVector(rhs.GetCount());
		for(int i=0; i<rhs.GetCount(); i++)
		{
			ret[i] = lhs * rhs[i];
		}

		return ret;

	}

	double operator * (const ColumnVector& lhs,const ColumnVector& rhs)
	{
		if(lhs._count != lhs._count)
			throw(new std::exception("lhs's count must be equal to rhs's count."));

		double res = 0;
		for(int i = 0; i < lhs._count; i++)
			res += lhs._datas[i] * rhs._datas[i];

		return res;
	}

	ColumnVector operator / ( const ColumnVector& lhs, const double rhs ) 
	{
		ColumnVector tmp(lhs.GetCount());

		for(int i = 0; i < lhs.GetCount(); i++)
			tmp[i] = lhs[i] / rhs;

		return tmp;
	}

	ColumnVector CrossProd(const ColumnVector& lhs,const ColumnVector& rhs)
	{
		if((lhs._count != 3) || (rhs._count != 3))
			throw(new std::exception("lhs's count must be equal to rhs's count, and must be 3."));

		ColumnVector tmp = ColumnVector(3);
		tmp[0] = lhs[1] * rhs[2] - lhs[2] * rhs[1];
		tmp[1] = lhs[2] * rhs[0] - lhs[0] * rhs[2];
		tmp[2] = lhs[0] * rhs[1] - lhs[1] * rhs[0];
		return tmp;
	}

	int ColumnVector::GetCount() const
	{
		return _count;
	}

	ColumnVector ColumnVector::GetSubColumnVector(const int startRow, const int endRow) const
	{
		if(startRow < 0)
			throw(new std::exception("startRow must be >= 0."));
		if(endRow >= _count)
			throw(new std::exception("endRow must be < count."));
		if(startRow > endRow)
			throw(new std::exception("startRow must be <= endRow."));

		ColumnVector retVector = ColumnVector(endRow - startRow);
		for(int i=0; i<retVector._count; i++)
		{
			retVector[i] = (*this)[i+startRow];
		}
		return retVector;
	}

	Matrix ColumnVector::Transpose()
	{
		Matrix ret = Matrix(1,this->_count);
		for(int i=0; i<_count; i++)
		{
			ret(0,i) = _datas[i];
		}

		return ret;
	}

	void ColumnVector::Normalize()
	{
		double v = this->Norm();
		if (v <= 0) 
		{
			throw(new std::exception("This vector's norm is <= 0."));
		}

		*this = (*this)/v;
	}

	double ColumnVector::Norm() const
	{
		// do some effort not to lose precision
		double tmp = 0;
		for(int i=0; i<_count; i++)
		{
			tmp = sqr(this->_datas[i]);
		}

		return sqrt(tmp);
	}


	void ColumnVector::SetZero()
	{
		for(int i=0; i<_count; i++)
		{
			_datas[i] = 0;
		}
	}

	ColumnVector Integ_Trap(const ColumnVector & present, ColumnVector & past, const double dt)
	{
		ColumnVector integration(present.GetCount());
		integration = (past+present)*0.5*dt;
		past = present;
		return integration;
	}

	bool Equal(const ColumnVector& lhs,const ColumnVector& rhs,const double eps)
	{
		if(lhs.GetCount() != rhs.GetCount())
			throw(new std::exception("lhs's count must be equal to rhs's count."));

		for(int i = 0; i < lhs.GetCount(); i++)
		{
			if(Equal(lhs[i],rhs[i],eps) == false)
				return false;
		}

		return true;

	}
}}